//
//  KalmanF.hpp
//  markerARLib
//
//  Created by fg on 6/21/16.
//  Copyright © 2016 CvidKal. All rights reserved.
//

#ifndef KalmanF_hpp
#define KalmanF_hpp

#include <opencv2/opencv.hpp>
#include <Eigen/Dense>
#include "Motion_Model.hpp"
#include "Basic Structure.h"

namespace markerAR {
    

class MotionFilter {
public:
    MotionFilter(){kf.init(12, 12,CV_64F);};

    Eigen::Matrix<double, 6, 1> addMotion(const RigidMotion&m,double timeStampNow,double f,double sigma_pixel,double meanReprojectError)
    {
        double time = timeStampNow - timeStamp;
        setTransitionMatrix(time);
        setProcessCov(time, 1e-3);
        setMeasurementMatrix();
        setMeasurementCov(f, sigma_pixel, meanReprojectError);

        kf.predict();

        cv::Mat measurement(12,1,CV_32F);
        for(int i=0;i<3;++i)
        {
            auto r = m.pose.EulerAngle();
            auto t = m.pose.t;
            auto tv = m.translationMotion.tv;
            auto rv = m.rotationMotion.rv;
            measurement.at<float>(i,0) = t(i);
            measurement.at<float>(i+3,0) = r(i);
            measurement.at<float>(i+6,0) =tv(i);
            measurement.at<float>(i+9,0) =rv(i);
        }
        cout<<"tv"<<m.translationMotion.tv;
        
        
        kf.correct(measurement);
        auto ret = kf.statePost;
        timeStamp = timeStampNow;
        return Converter::CV2Eigen<double,float,6, 1>(ret);
    }
    void initState(const RigidMotion&m,double timestamp)
    {
        timeStamp = timestamp;
        cout<<timeStamp<<endl;
        auto r = m.pose.EulerAngle();
        kf.statePre = Mat::zeros(kf.statePost.size(),kf.statePost.type());
        for(int i=0;i<3;++i)
        {
            kf.statePost.at<float>(i,0) = m.pose.t(i);
            
            kf.statePost.at<float>(i+3,0) = r(i);
        }
        kf.errorCovPost =Mat::zeros(kf.errorCovPost.size(),kf.errorCovPost.type());
        setMeasurementMatrix();
    }


    
private:
    
    void setMeasurementMatrix()
    {
        kf.measurementMatrix = Mat::zeros(kf.measurementMatrix.size(),kf.measurementMatrix.type());
        for (int i=0; i<12; ++i) {
            kf.measurementMatrix.at<float>(i,i) = 1;
        }
        
    }
    void setTransitionMatrix(double time)
    {
        kf.transitionMatrix = Mat::zeros(kf.transitionMatrix.size(),kf.transitionMatrix.type());
        for(int i=0;i<6;i++)
        {
            kf.transitionMatrix.at<float>(i,i) = 1;
            kf.transitionMatrix.at<float>(i,6+i) = time;
        }
    };
    void setProcessCov( double time, double sigma)
    {
        kf.processNoiseCov = Mat::zeros(kf.processNoiseCov.size(), kf.processNoiseCov.type());
        for (int i=0; i<6; ++i) {
            kf.processNoiseCov.at<float>(i,i) = pow(time,4)*pow(sigma,2);
            kf.processNoiseCov.at<float>(6+i,6+i) = pow(time,2)*pow(sigma,2);
        }
    }
    
    void setMeasurementCov(double f,double sigma_pixel,double meanReprojectError)
    {
        kf.measurementNoiseCov = Mat::zeros(kf.measurementNoiseCov.size(),kf.measurementNoiseCov.type());
        double sigma2 = (sigma_pixel*sigma_pixel + meanReprojectError*meanReprojectError)/f/f;
        cout<<sigma2<<endl;
        for(int i=0;i<6;++i)
        {
            kf.measurementNoiseCov.at<float>(i,i) = sigma2;
            kf.measurementNoiseCov.at<float>(6+i,6+i) = sigma2*4;

        }
    }
    
    cv::KalmanFilter kf;
    double timeStamp;
};

}

#endif /* KalmanF_hpp */
